#include <stdio.h>
#include <stdlib.h> //malloc
#include <string.h> //memcpy
#include <unistd.h> //sleep
#include"res_queue.h"

#define IDLE_FLAG_BIT  (0x80000000) //空闲状态flag,用来识别重复释放,增强队列健壮性
#define likely(x)   __builtin_expect(!!(x), 1)
#define unlikely(x) __builtin_expect(!!(x), 0)

/*
 * 初始化资源管理Queue
 */
res_queue_t* queue_res_init(unsigned int size)
{
    unsigned int i = 0;
    res_queue_t* q = (res_queue_t*)malloc(
    sizeof(res_queue_t) + sizeof(unsigned int) * 2 * size);
    if (q == NULL) return NULL;
    
    q->q_size = size;
    q->cnt    = 0;
    q->w      = 0;
    q->r      = 0;
    LOCK_INIT(q->lock_r, 0);
    LOCK_INIT(q->lock_w, 0);
    
    for (i = 0; i < size; i++) {
        q->data[i] = i|IDLE_FLAG_BIT;
        q->data[size+i] = i;
    }

    return q;
}
/*
 * 申请资源
 */
unsigned int queue_res_apply(res_queue_t* q)
{
    unsigned int rv = -1;
    if (unlikely(q == NULL)) return rv;

    LOCK(q->lock_w);
    
    if (likely(q->cnt != q->q_size)) {
        q->data[q->w] &= ~IDLE_FLAG_BIT;
        rv = q->data[q->w];
        q->w = (q->w + 1) % q->q_size;
        __sync_add_and_fetch(&q->cnt, 1);
    }
    
    UNLOCK(q->lock_w);
	//printf("[%s][%d]Queue w index=%d cnt=%d\n", __FUNCTION__, __LINE__, rv, q->cnt);
    return rv;
}

static inline void swap(unsigned int *a, unsigned int *b, unsigned int *c, unsigned int *d)
{
    *a = *a ^ *b;
    *b = *a ^ *b;
    *a = *a ^ *b;
    *c = *c ^ *d;
    *d = *c ^ *d;
    *c = *c ^ *d;
}
/*
 * 释放资源
 */
#define POS(x) q->data[q->q_size+x]
unsigned int queue_res_release(res_queue_t* q, unsigned int id)
{
    unsigned int rv = -1;
    if (unlikely(q == NULL || id >= q->q_size)) return rv;
    
    LOCK(q->lock_r);
    if (0 == (IDLE_FLAG_BIT & q->data[q->data[q->q_size+id]])) {
        if (likely(q->data[q->r] != id)) {
            // swap q->data[q->r]  q->data[POS(id)]的值
            __asm__ volatile (
            "xorl %1, %0\n\t"  // a = a ^ b
            "xorl %0, %1\n\t"  // b = a ^ b
            "xorl %1, %0\n\t"  // a = a ^ b
            "xorl %2, %3\n\t"  
            "xorl %3, %2\n\t"  
            "xorl %2, %3"      
            : "+r" (q->data[q->r]), "+r" (q->data[POS(id)]), "+r" (POS(q->data[q->r])), "+r" (POS(id)));
            //swap(&q->data[q->r], &q->data[POS(id)], &POS(q->data[q->r]), &POS(id));
        }

        q->data[q->r] |= IDLE_FLAG_BIT;
        q->r = (q->r + 1) % q->q_size;
        __sync_sub_and_fetch(&q->cnt, 1);
    }
    UNLOCK(q->lock_r);
    
    return 0;
}

void dump_res_queue(res_queue_t* q)
{
    if (q == NULL) return;
    
    printf("\nQ size = %u ,cnt = %u\n", q->q_size, q->cnt);
    printf("     r = %u\n", q->r);
    printf("     w = %u\n", q->w);
    #if(USE_FREE_LOCK)
    printf("r_lock = %u\n", q->lock_r);
    printf("w_lock = %u\n", q->lock_w);
    #endif
    unsigned int i = 0;
    unsigned int*idx = &q->data[q->q_size];
    /*
    for (i = 0; i < q->q_size; i++) {
        if (q->data[i] & IDLE_FLAG_BIT)
            printf("  %04u ", q->data[i] & (~IDLE_FLAG_BIT));
        else
            printf(" [%04u]", q->data[i]);
        if ((i + 1) % 20 == 0) printf("\n");
    }
    printf("\nidx:\n");
    for (i = 0; i< q->q_size; i++) {
        printf ("  %04u ", idx[i]);
        if((i + 1) % 20 == 0) printf("\n");
    }*/
    for (i = 0; i< q->q_size; i++) {
        if ((q->data[idx[i]] & (~IDLE_FLAG_BIT)) != i) {
            printf("ERR: idx[%u] = %u, q->data[%u]=%u\n", i, idx[i], idx[i], q->data[idx[i]]);
        }
    }
    printf("\n");
    return;
}